#include <mc_ros/sensors/sensor_config.h>

SensorConfig::SensorConfig():mode_ptr_(nullptr){
    paramsInit();

    mode_ptr_ =  std::make_shared<ModelBase>(robot_config_file_);

    if(!loadSnsorConfig()){
        ros::shutdown();
        return;
    }

}

// 参数初始化
void SensorConfig::paramsInit(){
    base_frame_ = ros::param::param<string>("/base_frame", "base_link");
    // string default_params_dir = ros::param::param<string>(
    //     "default_params_dir", "/home/xxty/trobot/trobot_ros1/config/params");
    // sensor_config_file_ = default_params_dir + "/sensor_params.yaml";

    sensor_config_file_ = ros::param::param<string>(
        "sensor_params_file", "/home/orangepi/trobot/trobot_ros1/config/params/orangepi/sensor_params.yaml");
    robot_config_file_ = ros::param::param<string>(
        "robot_params_file", "/home/orangepi/trobot/trobot_ros1/config/params/orangepi/robot_params.yaml");
}

// 加载传感器配置 yaml文件
bool SensorConfig::loadSnsorConfig(){

    // 参数解析
    auto getParams = [](YAML::Node item) -> SensorParams {
        SensorParams sensor_params;
        if(item["id"])
            sensor_params.sensor_base.id = item["id"].as<int>();
        if(item["sensor_frame"])
            sensor_params.sensor_base.sensor_frame = item["sensor_frame"].as<string>();
        if(item["topic_name"])
            sensor_params.sensor_base.topic_name = item["topic_name"].as<string>();
        if(item["camera_info_topic_name"])
            sensor_params.sensor_base.camera_info_topic_name = item["camera_info_topic_name"].as<string>();
        if(item["enable"])
            sensor_params.sensor_base.enable = item["enable"].as<bool>();
        if(item["tf_x"])
            sensor_params.transform.x = item["tf_x"].as<double>();
        if(item["tf_y"])
            sensor_params.transform.y = item["tf_y"].as<double>();
        if(item["tf_z"])
            sensor_params.transform.z = item["tf_z"].as<double>();
        if(item["tf_roll"])
            sensor_params.transform.roll = item["tf_roll"].as<double>();
        if(item["tf_pitch"])
            sensor_params.transform.pitch = item["tf_pitch"].as<double>();
        if(item["tf_yaw"])
            sensor_params.transform.yaw = item["tf_yaw"].as<double>();   
        return sensor_params;     
    };

    try{
        // 读取配置文件
        YAML::Node config = file_manager::ReadYamlFile(sensor_config_file_);
        if(config.IsNull()){
            LOG(FATAL) << "load file error! file_path is " 
                << sensor_config_file_.c_str();
            return false;
        }
        if(config["sensor_config"]){
            auto sensor_config = config["sensor_config"];
            for(auto iter = sensor_config.begin(); iter != sensor_config.end(); ++iter){
                string config_type = iter->first.as<string>();
                auto config_list = iter->second["config_list"];
                LOG(INFO) << "config_type: " << config_type << ", config_list: " << config_list << endl;
                // 遍历传感器列表    
                for(auto list_iter : config_list){
                    string sensor_type = list_iter.as<string>();
                    SensorParams sensor_params = getParams(iter->second[sensor_type]);
                    sensor_params.transform.base_frame = base_frame_;
                    LOG(INFO) << "--> sensor_type: " << sensor_type << ", config: " << toString(sensor_params) << endl; 
                    // 按传感器类型区分
                    if(config_type == "imu_config"){
                        imu_params_map_[sensor_params.sensor_base.id] = sensor_params;
                    }
                    else if(config_type == "lidar_config"){
                        lidar_params_map_[sensor_params.sensor_base.id] = sensor_params;
                    }
                    else if(config_type == "depth_camera_config"){
                        depth_params_map_[sensor_params.sensor_base.id] = sensor_params;
                        LOG(INFO) << "--> sensor_type: " << sensor_type 
                            << ", camera_info_topic_name: " << sensor_params.sensor_base.camera_info_topic_name;
                    }
                }
            }
        }

        if(config["sensor_filter_config"]){
            auto item = config["sensor_filter_config"];
            if(item["top_limit"])
                filter_config_.top_limit = item["top_limit"].as<double>();
            if(item["botton_limit"])
                filter_config_.botton_limit = item["botton_limit"].as<double>();
            if(item["ground_above"])
                filter_config_.ground_above = item["ground_above"].as<double>();
            if(item["ground_below"])
                filter_config_.ground_below = item["ground_below"].as<double>();
        }
        LOG(INFO) << "--> sensor_filter_config: " << toString(filter_config_);
    }
    catch(const YAML::Exception& error){
        LOG(ERROR) << error.what();
    }
    return true;
}


// 打印参数配置
string SensorConfig::toString(SensorParams sensor_params){
    std::ostringstream oss;
    oss << "[id: " << sensor_params.sensor_base.id;
    oss << ", enable: " << sensor_params.sensor_base.enable;
    oss << ", topic_name: " << sensor_params.sensor_base.topic_name;
    oss << ", sensor_frame: " << sensor_params.sensor_base.sensor_frame;
    oss << ", base_frame: " << sensor_params.transform.base_frame;
    oss << ", tf: (" << sensor_params.transform.x;
    oss << ", " << sensor_params.transform.y;
    oss << ", " << sensor_params.transform.z;
    oss << ", " << sensor_params.transform.roll;
    oss << ", " << sensor_params.transform.pitch;
    oss << ", " << sensor_params.transform.yaw;
    oss << ")]";
    return oss.str();
}

// 打印过滤配置
string SensorConfig::toString(SensorFilterConfig filter_config){
    std::ostringstream oss;
    oss << "[top_limit: " << filter_config.top_limit;
    oss << ", botton_limit: " << filter_config.botton_limit;
    oss << ", ground_above: " << filter_config.ground_above;
    oss << ", ground_below: " << filter_config.ground_below;
    oss << "]";
    return oss.str();
}

// 获取雷达配置
vector<SensorParams> SensorConfig::getLidarParams(){
    vector<SensorParams> params_list;
    if(!lidar_params_map_.empty()){
        for(auto iter = lidar_params_map_.begin(); iter != lidar_params_map_.end(); ++iter){
            params_list.push_back(iter->second);
        }
    }
    return params_list;
}

// 获取相机配置
vector<SensorParams> SensorConfig::getDepthCameraParams(){
    vector<SensorParams> params_list;
    if(!depth_params_map_.empty()){
        for(auto iter = depth_params_map_.begin(); iter != depth_params_map_.end(); ++iter){
            params_list.push_back(iter->second);
        }
    }
    return params_list;
}

// 获取IMU配置
vector<SensorParams> SensorConfig::getIMUParams(){
    vector<SensorParams> params_list;
    if(!imu_params_map_.empty()){
        for(auto iter = imu_params_map_.begin(); iter != imu_params_map_.end(); ++iter){
            params_list.push_back(iter->second);
        }
    }
    return params_list;
}

// 获取机器人配置信息
RobotModel SensorConfig::getRobotModel(){
    return mode_ptr_->getRobotModel();
}